#pragma once
#include <Shape2D.h>
#include <stack>
#include <unordered_map>
#include <queue>
#include <CollisionEntity.h>
#include <unordered_set>
struct NavTriangle;
struct NavPathEdge
{
    Vector2 p1, p2;
};
struct Edge
{
    std::vector<NavTriangle *> tris;
    bool isRestrain = false;
    bool isBad = false;
    Vector2 p1, p2;
    Edge(Vector2 p1, Vector2 p2);
    Edge() = default;
    Edge(const Edge &e) = default;
    bool equal(const Edge &e) const;

public:
    NavPathEdge *createNavPathEdge();
};

struct NavPathTriangle
{
    int cost;
    int h;
    int g;
    NavPathEdge *nextEdge = nullptr;
    NavPathTriangle *from = nullptr;
    NavTriangle *parent = nullptr;
};

struct NavTriangle : public Triangle
{
    std::vector<Edge *> edges;
    NavTriangle(Vector2 p1, Vector2 p2, Vector2 p3);
    NavTriangle(const NavTriangle &tri) = default;

public:
    NavPathTriangle *createNavPathTriangle();
};

struct NavPathTriangleCompare
{
    bool operator()(NavPathTriangle *a, NavPathTriangle *b)
    {
        return a->cost > b->cost;
    }
};

class Navmesh
{
private:
    std::vector<Vector2> plist;
    std::vector<Edge> elist;
    std::vector<NavTriangle *> trilist;
    std::vector<Edge *> edgelist;
    std::vector<Vector2> boundVertexs;

public:
    Navmesh(/* args */) = default;
    ~Navmesh();
    void clear();
    void generate();
    real static getTriangleMinAngle(const Triangle *tri);
    const std::vector<NavTriangle *> *getTriangles() const;
    int getContainTrigleIndex(Vector2 p);
    NavTriangle *getContainTrigle(Vector2 p) const;
    void addRestrainEdge(Edge e);
    void addRestrainPolygon(const std::vector<Vector2> ps);
    const std::vector<Edge *> *getEdge() const;
    void addPoint(Vector2 p);
    void setBoundVertexs(const std::vector<Vector2> ps);
    std::vector<NavPathTriangle *> findPathTriangles(Vector2 start, Vector2 end, real radius = 0);
    std::stack<Vector2> findPath(CollisionEntity *agent, Vector2 target);
    Edge *getCommonEdge(NavTriangle *t1, NavTriangle *t2);
    std::unordered_set<const Edge *> pathDetection(Vector2 from, Vector2 to);
    std::unordered_set<const Edge *> widthpathDetection(Vector2 from, Vector2 to, real width, int max = 5);

private:
    Vector2 _findTurnPoint(Vector2 ori, Vector2 target, const std::vector<NavPathTriangle *> &list, int &index, std::vector<int> *crossTriangles = nullptr, bool *turnLeft = nullptr);
    void _triangulate(std::vector<Vector2> ps);
};